//
// Created by sun on 2020/6/13.
//

#ifndef SRC_OFFBOARDCORE_H
#define SRC_OFFBOARDCORE_H

#include "ros/ros.h"
#include "serial/serial.h"
#include <string>
#include <vector>
#include <iostream>
#include <ctime>
#include <ros/package.h>
#include <std_msgs/Bool.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/PositionTarget.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/ExtendedState.h>
#include <fstream>
#include "tf/transform_datatypes.h"
#include <eigen3/Eigen/Core>
#include "px4_gcs/swarm_state.h"
#include "Utils/TrajManager.h"
#include "Utils/CSVManager.h"
#include "Utils/RosUtils.h"

using namespace std;

#define PI 3.141593


enum air_state {
    in_air,
    landing,
    landed,
    takeoffing,
    waitting_takeoff
};

enum arm_state {
    armed,
    disarmed
};

enum fly_mode {
    offboard,
    manual,
    altctl,
    posctl,
    stabilized
};

enum control_methord {
    pos,
    vel,
    att
};

enum position_state {
    using_vicon,
    using_gps,
    using_att,
};

enum run_state {
    run_takeoff,
    run_traj,
    run_hold_traj_end,
    run_hold,
    run_land
};

struct current_state {
    arm_state armState;
    air_state airState;
    control_methord controlMethord;
    position_state positionState;
    fly_mode flyMode;
    run_state currentRunState;
    run_state nextRunState;
    bool current_run_finished;
};


class OffboardCore {
public:
    string node_name;
    vector<vector<float>> traj;// [i][0-3]: timestamp, x, y, z
    vector<vector<float>> resampled_traj;
    string readfilepath;
    string recordfilename;
    string recordfilepath;
    string mavros_id;
    string pkg_path;
    string local_pose_topic_name;
    string publish_pose_topic_name;

    mavros_msgs::PositionTarget sp_pos;
    current_state state{};
    tf::Quaternion cur_att;

    ros::Subscriber vision_pose_sub;
    ros::Subscriber local_vel_sub;
    ros::Subscriber mode_sub;
    ros::Subscriber lan_sub;
    ros::Subscriber swarm_state_sub;
    vector<ros::Subscriber> swarm_ready_sub;
    vector<bool> swarm_ready_bool;
    ros::Publisher sp_raw_local_pub;
    ros::Publisher current_drone_ready_pub;
    ros::ServiceClient cmd_request;
    mavros_msgs::SetMode mode_cmd;
    mavros_msgs::CommandBool arm_cmd;
    mavros_msgs::ExtendedState land_state;
    std_msgs::Bool current_drone_ready_msg;
    ros::ServiceClient arming_client;
    ros::Subscriber local_pose_sub;

    float cur_pos[3] = {0.0, 0.0, 0.0};
    float record_pos[3] = {0.0, 0.0, 0.0};
    float cur_vel[3] = {0.0, 0.0, 0.0};
    float cur_angle[3] = {0.0, 0.0, 0.0}; // roll p
    float init_pose[3] = {0.0, 0.0, 0.0};
    float sp_rpyt[4] = {0.0, 0.0, 0.0, 0.0};
    float yaw_rate = 0;
    float takeoff_height = 0;
    float land_height = 0;
    float global_init_pose_x = 0;
    float global_init_pose_y = 0;
    float land_speed = 0.3;
    float rect_x = 1;  //矩形x边长
    float rect_y = 1;  //矩形y边长
    float traj_scale = 1;
    float traj_scale_x = 1;
    float traj_scale_y = 1;
    float traj_scale_z = 1;
    float cur_yaw = 0.0;
    float init_yaw = 0.0;
    float target_height = 0.0;
    float _target[3] = {0.0, 0.0, 0.0};
    float offset_z = 5.0;
    float offset_y = 0.0;
    float cur_err_dist = 0.0;

    bool isMaster = false;
    bool traj_ready = false;
    bool swarm_state_changed = false;
    bool init_process_finished = false;
    bool take_off_finished = false;
    bool land_finished = false;
    bool record_data = false;
    bool fly_traj = true;
    bool traj_finished = false;
    bool fly_finished = false;
    bool using_gazebo = false;
    bool use_traj_z = false;
    bool use_resample = false;
    bool use_takeoff = true;
    bool use_land = true;
    bool use_local_head_target = false;

    int use_enu = 1;//int 1 use_enu 0 use ned
    int re_pub_pos = 0;//int 1 use_enu 0 use ned
    int using_position_methord = 0;//int 1 using_vicon 2 using_gps 3 using_att
    int using_control_mode = 0; // 0 position 1 vel 2 att
    int id = 0;
    int swarm_num = 1;
    int rate = 30;
    int move_stage = 0;
    int control_count_time = 0;
    int control_time = 60;
    int traj_speed = 1;
    int traj_speed_control = 1;
    int resample_num = 0;

    vector<int> nodes;
    vector<int> active;
    vector<int> ready;

    OffboardCore() = default;

    OffboardCore(ros::NodeHandle &n);

    void initRosSubPubManager(ros::NodeHandle &n);

    bool readCSV(const string &filename, vector<vector<float>> &traj);

    void updateParams(ros::NodeHandle &nh);

    void chatterCallback_local_pose(const geometry_msgs::PoseStamped &msg);

    void chatterCallback_local_vel(const geometry_msgs::TwistStamped &msg);

    void chatterCallback_extend_state(const mavros_msgs::ExtendedState &msg);

    void chatterCallback_mode(const mavros_msgs::State &msg);

    void takeoff(float height);

    void holdInitPos(float height);

    void land(float target_x, float target_y);

    void land();

    void holdZeroPosition();

    void initState();

    void load_traj();

    void loadTrajFromVector(vector<vector<float>> &traj);

    void moveToRelativeNow(float dx, float dy, float dz);

    void moveto(float target_x, float target_y, float target_z,
                float base_x, float base_y, float base_z);

    inline float getDistanceFrom(float x, float y, float z);

    void fly_traj_count_base(float base_x, float base_y, float base_z, int _control_count);

    void fly_traj_base(float base_x, float base_y, float base_z);

    // 把traj的轨迹点数由原来的size个减小为num个 num应该和size 差不多大
    void resample(int num);

    void fly_rect();

    void process_run();

    void try_ready_takeoff();

    void dronei_ready_cb(const std_msgs::Bool::ConstPtr &msg, int &i);

    void rotate_yaw(float yaw);

    void setDisarmed();

    void setArmed();

    void setAltctl();

    void setStabilized();

    void setPosctl();

    void setOffboard();

    void record_pose();

    bool swarm_ready();

    void setMannual();

    string printState(run_state state);

    template<typename T>
    T readParam(ros::NodeHandle &n, std::string name);

    void init_position(ros::Rate &loop_rate, int average_num);

    void mainLoop();

private:

    void swarm_state_cb(const px4_gcs::swarm_state &msg);
};


#endif //SRC_OFFBOARDCORE_H
